#include "BulletSimulatorThread.h"

#include "BulletSimulator.h"
#include "BasicDebug.h"

#include <QtCore/QDateTime>
#include <iostream>
namespace jcrada {

    BulletSimulatorThread::BulletSimulatorThread( ) : QThread( ),
    _simulator( NULL ),
    _behavior_enabled( false ),
    _behavior_on_ground( false ),
    _evaluation_time_enabled( false ),
    _evaluation_time_in_seconds( 10 ),
    _simulation_mode( SM_FREE ) {
        setSimulator(&BulletSimulator::getSingleton());
    }

    BulletSimulatorThread::~BulletSimulatorThread( ) {
        BulletSimulator::destroySingleton();
    }

    void BulletSimulatorThread::setSimulator( BulletSimulator* _simulator ) {
        if (this->_simulator) {
            QObject::disconnect(this->_simulator, SIGNAL(changedPlay(bool)),
                    this, SLOT(notify()));
        }
        this->_simulator = _simulator;
        QObject::connect(this->_simulator, SIGNAL(changedPlay(bool)),
                this, SLOT(notify()), Qt::DirectConnection);
    }

    void BulletSimulatorThread::notify( ) {
        if (!this->isRunning()) {
            start();
        }
    }

    void BulletSimulatorThread::stop( ) {
        getSimulator().play(false);
        wait();
    }

    void BulletSimulatorThread::run( ) {
        switch (getSimulationMode()) {
            case SM_FREE: _run_free();
                break;
            case SM_EVALUATION: _run_evolution();
                break;
            default:
                BDEBUG(TO_STRING(getSimulationMode()));
                assert(0);
        }
    }

    void BulletSimulatorThread::_run_free( ) {
        bool ground_was_hit = getSimulator().hasHitTheGround();
        QTime time;
        time.start();
        while (getSimulator().isPlaying() && getSimulationMode() == SM_FREE) {
            time.restart();
            step(ground_was_hit);
            waitForTimestepsCompletion(time.elapsed());
        }
        getSimulator().getLock().lockForRead();
        if (getSimulator().currentCreature()) {
            getSimulator().currentCreature()->beforeRemove(getSimulator());
        }
        getSimulator().getLock().unlock();
    }

    void BulletSimulatorThread::_run_evolution( ) {
        int max_timesteps = getEvaluationTimeInSeconds() *
                getSimulator().getTimestepsPerSecond();
        bool ground_was_hit = false;
        int current_timestep = 0;
        QTime time;
        time.start();
        while (getSimulationMode() == SM_EVALUATION &&
                current_timestep < max_timesteps) {
            time.restart();
            step(ground_was_hit);
            if (ground_was_hit) {
                ++current_timestep;
            }
            waitForTimestepsCompletion(time.elapsed());
        }
        getSimulator().getLock().lockForRead();
        if (getSimulator().currentCreature()) {
            getSimulator().currentCreature()->beforeRemove(getSimulator());
        }
        getSimulator().getLock().unlock();
    }

    void BulletSimulatorThread::step( ) {
        getSimulator().play(false);
        bool ground_was_hit = getSimulator().hasHitTheGround();
        step(ground_was_hit);
    }

    void BulletSimulatorThread::step( bool& ground_was_hit ) {
        getSimulator().step();
        getSimulator().getLock().lockForRead();
        Creature* creature = getSimulator().currentCreature();
        if (creature) {
            if (!ground_was_hit && getSimulator().hasHitTheGround()) {
                ground_was_hit = true;
                creature->onHitTheGround(getSimulator());
//                DEBUG("CREATURE HIT GROUND!");
            }
            if (isBehaviorEnabled()) {
                 if (!isBehaviorOnGround() ||
                        (isBehaviorOnGround() && getSimulator().hasHitTheGround())) {
                    creature->onStep(getSimulator());
                }
            }
        }
        getSimulator().getLock().unlock();
    }

    void BulletSimulatorThread::waitForTimestepsCompletion( int time_elapsed_ms ) {
        long sleep = 0;
        if (!getSimulator().isFastForward()) {
            sleep = (1000.0 / getSimulator().getTimestepsPerSecond()) - time_elapsed_ms;
            if (sleep > 0) {
                this->msleep(sleep);
            } else {
                BDEBUG("Warning: Too many timesteps per second");
            }
        }
    }

    void BulletSimulatorThread::evaluate( Creature& creature ) {
        wait();
        getSimulator().setHitTheGround(false);
        getSimulator().addCreature(&creature);
        notify();
        wait();
        getSimulator().removeCreature(&creature);
    }

    //SLOTS...

    void BulletSimulatorThread::setBehaviorEnabled( bool enabled ) {
        if (this->_behavior_enabled != enabled) {
            this->_behavior_enabled = enabled;
            emit changedBehaviorEnabled(enabled);
        }
    }

    void BulletSimulatorThread::setBehaviorOnGround( bool on_ground ) {
        if (this->_behavior_on_ground != on_ground) {
            this->_behavior_on_ground = on_ground;
            emit changedBehaviorOnGround(on_ground);
        }
    }

    void BulletSimulatorThread::setEvaluationTimeInSeconds( int seconds ) {
        if (this->_evaluation_time_in_seconds != seconds) {
            this->_evaluation_time_in_seconds = seconds;
            emit changedEvaluationTimeInSeconds(seconds);
        }

    }

    void BulletSimulatorThread::setSimulationMode( int mode ) {
        if (this->_simulation_mode != mode) {
            this->_simulation_mode = (eSimulationMode) mode;
            emit changedSimulationMode(mode);
        }
    }
}
